// gripServoSerial
// DEPRACATED DO NOT USE!!!!!

#include <Servo.h>

#define MSG_LENGTH 5
#define MSG_TYPE_POS 0 // the servo type specified at the 0 position of msg
#define MSG_TYPE_SERVO 's'
#define MSG_TYPE_GRIP 'g'
#define MSG_TYPE_WRIST 'w'
#define MSG_END ';'

//int ledPin = 13; // led pin to turn on when computer input  has been received
//char inByteBuf[100]; // character buffer

int pServo = 0; // servo position
Servo gripServo; // define gripper servo object
int pGrip = 0; // position of gripper servo
Servo wristServo; // define wrist servo objec


boolean msgEnd = false;
int msgPtr = 0; // index of incoming character array
char msg[MSG_LENGTH] = {'0','0','0','0','0'}; // incoming serial string from computer

void setup()
{
  // start serial port at 9600 bps:
  Serial.begin(9600);
  establishContact(); // send a byte to establish contact until
                      // receiver responds                                      
                      
  //pinMode(ledPin,OUTPUT); // led pin to flash on and off as confirmation
  Serial.println();
  
  gripServo.attach(9);  
  resetServos();      // set to zero, 180 and then 90 deg position
  delay(1000);
  //gripServo.write(0); // set digital servo to zero position
}


void loop() 
{ 
  
  // receive input from computer
  // if we get a valid byte, read input
  while ((Serial.available() > 0) && !msgEnd && msgPtr < MSG_LENGTH) 
  {
    Serial.println("Reading msg");
    
    msg[msgPtr] = Serial.read(); // get incoming byte (a single character)
    //String msgStr = String(msg);
    
    if (msg[msgPtr] == MSG_END)
    {
      msgEnd = true;
      //Serial.print(msgPtr);
      //Serial.println();
      //Serial.print(msg);
      
    } else {
      
      msgEnd = false;
      //Serial.print(msgPtr);
      //Serial.println();
      
      msgPtr++;
      
      //Serial.print(msg);
    }
  }    
    
    if (msgEnd == true)
    {
      
      Serial.println("Message received: ");
      Serial.print(msg);
      Serial.println();
 
      Serial.println("Parsing msg");
      
      char type = msg[MSG_TYPE_POS]; // determine what type of msg it is/for which servo 
      Serial.print(type);
      Serial.println();
      
      if (type == MSG_TYPE_GRIP)
      {

        int pGrip1 = atoi(&msg[1]);
        //int pGrip2 = atoi(&msg[2]);
        //int pGrip3 = atoi(&msg[3]);
        //Serial.println();
        //Serial.print(pGrip1);
        //Serial.print(pGrip2);
        //Serial.print(pGrip3);
        //Serial.println();
        pGrip = pGrip1;
        
        //Serial.print(pGrip);
        //Serial.println();
        //pGrip = map(pGrip,0,9,1,179);
        
        Serial.print("Writing to grip servo: ");
        Serial.print(pGrip);
        Serial.println();
        
        gripServo.write(pGrip);
        
      // if not g s e or w, wrong servo ID
      
      } else if (type == MSG_TYPE_WRIST)
      {
        int pWrist = atoi(&msg[1]);
        Serial.print(pWrist);
        Serial.println();
        pWrist = map(pWrist,0,9,1,180);
        
        Serial.print("Writing to wrist servo: ");
        Serial.print(pWrist);
        Serial.println();
        
        gripServo.write(pWrist);
        
      } else {
        Serial.print("ERROR: Unknown msg type - ");
        Serial.print("Message: ");
        Serial.println(msg);
      }
      
      
    }
    
    //digitalWrite(ledPin, HIGH); // set LED on
    //delay(100);
    //digitalWrite(ledPin, LOW); // set LED off
    
    // convert character to numerical position
    //inString.toCharArray(inByteBuf,90);
    //pServo = atoi(inByteBuf);
    
    /*
    // print out position
    Serial.println("Str to Int: ");
    Serial.print(pServo);
    Serial.println();
    
    pServo = pServo; // to bring pServo relative to zero, ! = ASCII 33
    
    Serial.println("Position of Servo Sent: ");
    Serial.print(pServo);
    Serial.println();
    
    // move servo
    //gripServo.write(pServo);
    
        // send value back to computer
    */
    
    // reset variables for loop
      msgEnd = false;
      msgPtr = 0;
      for (int i = 0; i < MSG_LENGTH; i++) 
      {
        msg[i] = 0;
      }
      
    delay(1000); // wait for something...
 
  
  /*
  val = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023) 
  val = map(val, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180) 
  myservo.write(val);                  // sets the servo position according to the scaled value 
  delay(15);                           // waits for the servo to get there 
  */
} 

void establishContact() {
  while (Serial.available() <= 0) {
    Serial.print('A', BYTE);   // send a capital A
    delay(300);
  }
}

void resetServos() {
     gripServo.write(0);
     delay(1000);
     gripServo.write(179);
     delay(1000);
     gripServo.write(90); 
}
